#include <ros/ros.h>
#include <tf2/utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>

#include "path_generate/path_base.h"
#include "path_generate/p2p/astar.h"
#include "path_generate/p2p/jps.h"
#include "controller/controller_base.h"
#include "perception/costmap_2d/costmap_2d_ros.h"
#include "controller/pure_pursuit/pure_pursuit.h"
#include "core/robot_model/robot_model_base.h"
#include <mc_ros/visualization/controller_visualization.h>

class TestPathReplan{
    public:
        TestPathReplan(tf2_ros::Buffer* tf_buffer);
        ~TestPathReplan() = default;
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg);        
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStamped::Ptr& msg);
        // 融合数据回调函数
        void FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg);
        // 全局代价地图回调函数
        void CostmapGlobalCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // 局部代价地图回调函数
        void CostmapLocalCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // 循环控制
        void Loop();

    private:
        ros::NodeHandle nh_;
        tf2_ros::Buffer* const tfBuffer_;
        ros::Subscriber mapSub_; 
        ros::Subscriber lidarScanSub_;
        ros::Subscriber rvizPoseSub_;
        ros::Subscriber fusionDataSub_;
        ros::Publisher LookaheadPosePub_;
        ros::Subscriber costmapGlobalSub_;
        ros::Subscriber costmapLocalSub_;

        boost::shared_ptr<nav_msgs::OccupancyGrid> costmap_local_ptr_;     
        boost::shared_ptr<nav_msgs::OccupancyGrid> costmap_global_ptr_; 
        boost::shared_ptr<ControllerBase> localControllerPtr_; 
        boost::shared_ptr<sensor_msgs::PointCloud2> fusionDataPtr_;
        boost::shared_ptr<geometry_msgs::PoseStamped> rvizGoalPtr_;
        std::shared_ptr<PlannerBase> globalPathPlannerPtr_;

        boost::shared_ptr<ModelBase> robotModelPtr_;        //机器人模型
        boost::shared_ptr<MCVisualization> mcVisualizationPtr_;

        bool need_global_path_;
        vector<geometry_msgs::PoseStamped> pathGlobal_;
    };


TestPathReplan::TestPathReplan(tf2_ros::Buffer* tf_buffer):tfBuffer_(tf_buffer),
    fusionDataPtr_(nullptr), rvizGoalPtr_(nullptr), need_global_path_(true),
    costmap_local_ptr_(nullptr), costmap_global_ptr_(nullptr){
    
    string robotModelConfigFile_ = ros::param::param<string>(
        "robot_params_file", "/home/orangepi/trobot/trobot_ros1/config/params/orangepi/robot_params.yaml");
    robotModelPtr_ = boost::make_shared<ModelBase>(robotModelConfigFile_);
    mcVisualizationPtr_ = boost::make_shared<MCVisualization>();
    mcVisualizationPtr_->setRobotModel(robotModelPtr_);

    globalPathPlannerPtr_ = std::make_shared<JPSPlanner>();
    localControllerPtr_ = boost::make_shared<PurePursuit>();

    mapSub_ = nh_.subscribe("/map", 10, &TestPathReplan::MapCallback, this);
    costmapGlobalSub_ = nh_.subscribe("/global_costmap/costmap", 10, &TestPathReplan::CostmapGlobalCallback, this);
    costmapLocalSub_ = nh_.subscribe("/local_costmap/costmap", 10, &TestPathReplan::CostmapLocalCallback, this);
    rvizPoseSub_ = nh_.subscribe("/move_base_simple/goal", 10, &TestPathReplan::RvizPosesCallback, this);
    fusionDataSub_ = nh_.subscribe("/fusion_bev", 10, &TestPathReplan::FusionDataCallback, this);
    LookaheadPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/lookahead_pose", 10);
    Loop();
}

// 地图回调函数
void TestPathReplan::MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    LOG(INFO) << "Receive a map. width=" << msg->info.width 
        << ", height=" << msg->info.height
        << ", resolution=" << msg->info.resolution;
}

// 全局代价地图回调函数
void TestPathReplan::CostmapGlobalCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    costmap_global_ptr_ = boost::make_shared<nav_msgs::OccupancyGrid>(*msg);
    globalPathPlannerPtr_->setMap(costmap_global_ptr_); 
}

// 局部代价地图回调函数
void TestPathReplan::CostmapLocalCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    costmap_local_ptr_ = boost::make_shared<nav_msgs::OccupancyGrid>(*msg);
    localControllerPtr_->updateCostmapData(msg);
}

// Rviz位姿回调函数
void TestPathReplan::RvizPosesCallback(const geometry_msgs::PoseStamped::Ptr& msg){
    LOG(INFO) << "[RvizPosesCallback] Receive a pose. x=" << msg->pose.position.x
        << ", y=" << msg->pose.position.y
        << ", yaw=" << tf2::getYaw(msg->pose.orientation);
    rvizGoalPtr_ = msg;
    need_global_path_ = true;
}

// 融合数据回调函数
void TestPathReplan::FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg){
    fusionDataPtr_ = msg;
}

// 循环控制
void TestPathReplan::Loop(){
    ros::Rate controller_rate(20);  //20Hz
    nav_msgs::OccupancyGrid local_map;
    nav_msgs::OccupancyGrid global_map;
    while(!ros::isShuttingDown()){
        controller_rate.sleep();
        ros::spinOnce();
        LOG(INFO) << "--------------------------------";

        ros::Time now = ros::Time::now();
        if(!rvizGoalPtr_) continue;
        // 发布机器人模型
        mcVisualizationPtr_->publishRobotModel("base_link");

        // 获取机器人当前位姿
        geometry_msgs::PoseStamped robotPoseStamp;
        if(!localControllerPtr_->getRobotPose(tfBuffer_, robotPoseStamp)){
            LOG(WARNING) << "can not get robot pose!";
            continue;
        }
        localControllerPtr_->updateRobotPose(robotPoseStamp);

        // 是否需要生成全局路径
        if(need_global_path_ && rvizGoalPtr_){
            // 生成全局路径
            vector<geometry_msgs::Pose> poses;
            mc::status gen_state = globalPathPlannerPtr_->genPath(robotPoseStamp.pose, rvizGoalPtr_->pose, poses); 
            if(!gen_state.success || poses.empty()){
                LOG(WARNING) << "generate path failed!"; 
                continue; 
            }  

            pathGlobal_.resize(poses.size());
            for(size_t i = 0; i < poses.size(); ++i){
                pathGlobal_[i] = msg_convert::toPoseStamped(poses[i], "map");
            }
            localControllerPtr_->updateGlobalPath(pathGlobal_);
            mcVisualizationPtr_->publishGolbalPath(pathGlobal_);
            need_global_path_ = false;
        }

        // 检测障碍物，局部路径重规划
        if(pathGlobal_.empty()){
            continue;
        }

        try{
            // // 裁剪路径
            // localControllerPtr_->pruningPath(1.0f);
            // // 发布全局路径
            // vector<geometry_msgs::PoseStamped> path_poses
            //     = localControllerPtr_->getGlobalPath();
            // mcVisualizationPtr_->publishGolbalPath(path_poses);

            // // 获取前瞻点
            // geometry_msgs::PoseStamped lookahead_goal;
            // lookahead_goal.header.frame_id = "map";
            // int lookahead_goal_index = -1;
            // double lookahead_distance = localControllerPtr_->getLookAheadDistance(0.2);
            // if(!localControllerPtr_->getLookaheadGoal(lookahead_distance, lookahead_goal, lookahead_goal_index)){
            //     throw std::runtime_error("can not get lookahead goal!");
            // }
            // LOG(INFO) << "[Loop] lookahead_goal: " << msg_convert::toString(lookahead_goal.pose)
            //     << ", distance: " << lookahead_distance 
            //     << ", index: " << lookahead_goal_index;
            // LookaheadPosePub_.publish(lookahead_goal);

            // // 碰撞检测
            // int collision_index = -1;
            // if(localControllerPtr_->isPathCollision(collision_index)){
            //     LOG(WARNING) << "path collision!";

            //     // 重规划路径
            //     if(!localControllerPtr_->replanPath(collision_index)){
            //         throw std::runtime_error("replan path failed!");
            //     }
            // }
        }
        catch (const std::runtime_error& e) {
            std::cerr << "[Loop ] error: " << e.what() << std::endl;
        }   
    }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "test_path_replan");

    string glog_dir = ros::param::param<string>("/glog_dir", "/home/xxty/user_config/logs");
    common::glog::init(glog_dir, "test_path_replan");

    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    // ROS_INFO("test_path_generate start...");
    LOG(INFO) << "test_path_replan start...";

    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);
    TestPathReplan test_path_replan(&tfBuffer);
    ros::spin();

    common::glog::shutdown();

    return 0;
}
